/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 *  @file  testBearingRangeFactor.cpp
 *  @brief Unit tests for BearingRangeFactor Class
 *  @author Frank Dellaert
 *  @date July 2015
 */

#include <CppUnitLite/TestHarness.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/nonlinear/expressionTesting.h>
#include <gtsam/nonlinear/factorTesting.h>
#include <gtsam/sam/BearingRangeFactor.h>

using namespace std;
using namespace gtsam;

namespace {
Key poseKey(1);
Key pointKey(2);
}  // namespace

/* ************************************************************************* */
TEST(BearingRangeFactor, 2D) {
  typedef BearingRangeFactor<Pose2, Point2> BearingRangeFactor2D;
  SharedNoiseModel model2D(noiseModel::Isotropic::Sigma(2, 0.5));
  BearingRangeFactor2D factor2D(poseKey, pointKey, 1, 2, model2D);

  // Set the linearization point
  Values values;
  values.insert(poseKey, Pose2(1.0, 2.0, 0.57));
  values.insert(pointKey, Point2(-4.0, 11.0));

  EXPECT_CORRECT_EXPRESSION_JACOBIANS(factor2D.expression({poseKey, pointKey}),
                                      values, 1e-7, 1e-5);
  EXPECT_CORRECT_FACTOR_JACOBIANS(factor2D, values, 1e-7, 1e-5);
}

/* ************************************************************************* */
TEST(BearingRangeFactor, 3D) {
  typedef BearingRangeFactor<Pose3, Point3> BearingRangeFactor3D;
  SharedNoiseModel model3D(noiseModel::Isotropic::Sigma(3, 0.5));

  const Unit3 bearing = Pose3().bearing(Point3(1, 0, 0));
  const double range = 1.0;
  BearingRangeFactor3D factor3D(poseKey, pointKey, bearing, range, model3D);
  
  // Set the linearization point
  Values values;
  values.insert(poseKey, Pose3());
  values.insert(pointKey, Point3(1, 0, 0));

  // Check that the error is zero at the linearization point
  Vector actualError = factor3D.unwhitenedError(values);
  EXPECT(assert_equal(Vector::Zero(actualError.size()), actualError, 1e-9));

  // TODO(frank): this test is disabled (for now) because the macros below are
  // incompatible with the Unit3 localCoordinates. See testBearingFactor...
  // EXPECT_CORRECT_EXPRESSION_JACOBIANS(factor3D.expression({poseKey, pointKey}),
  //                                     values, 1e-7, 1e-5);
  // EXPECT_CORRECT_FACTOR_JACOBIANS(factor3D, values, 1e-7, 1e-5);
}  // namespace

/* ************************************************************************* */
int main() {
  TestResult tr;
  return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */
